//############################################################
// FILE: Task_function.c
// Created on: 2017年1月18日
// Author: XQ
// summary: Task_function
//本程序只供学习使用，未经作者许可，不得用于其它任何用途
//版权所有，盗版必究
//DSP/STM32电机控制开发板
//硕历电子
//网址: https://shuolidianzi.taobao.com
//Author-QQ: 616264123
//电机控制QQ群：314306105
//############################################################
#include "main_user.h"

#include "FOC_run.h"
#include "RS485.h"
#include "Timer.h"   
#include "ThreeHall.h"  
#include "IQ_math.h"
#include "Task_function.h"

#include "foc_pid.h"
#include "AS5600.h"
#include "Soft_IIC.h"

#include "SEGGER_RTT_Port.h"

void DWT_Init(void);
void Uart_485_Init(void);

 
Test         TestPare = Test_DEFAULTS;
TaskTime     TaskTimePare = TaskTime_DEFAULTS;
logic        logicContr = logic_DEFAULTS;
IQSin_Cos    AngleSin_Cos = IQSin_Cos_DEFAULTS ;



extern uint32_t start,end,cycles;
extern float time_us;



void main_user(void)
{
	//ADC校准
	HAL_ADCEx_Calibration_Start( &hadc1);
	HAL_ADCEx_Calibration_Start( &hadc2);
	__HAL_ADC_CLEAR_FLAG( &hadc1, ADC_FLAG_JEOC);
	__HAL_ADC_CLEAR_FLAG( &hadc2, ADC_FLAG_JEOC);
	//启动ADC1规则组的DMA连续转换
  HAL_ADC_Start_DMA(&hadc1, (uint32_t*)ADC_ConvertedValue, 2);

	
	__HAL_TIM_CLEAR_FLAG( &htim1, TIM_FLAG_BREAK);
	HAL_TIM_PWM_Start( &htim1, TIM_CHANNEL_4);
  HAL_TIM_Base_Start_IT(&htim1);  // 启动定时器1中断
		
	//__HAL_TIM_CLEAR_FLAG( &htim4, TIM_FLAG_BREAK);
	//HAL_TIM_Encoder_Start( &htim4, TIM_CHANNEL_ALL);

  //485初始化
	Uart_485_Init();
	RS485_StartReceive();
	__HAL_UART_ENABLE_IT( &huart3, UART_IT_IDLE);
	__HAL_UART_CLEAR_IDLEFLAG( &huart3);
	__HAL_UART_DISABLE_IT( &huart3, UART_IT_TC);
	__HAL_UART_CLEAR_FLAG( &huart3, UART_FLAG_TC);
	
	/******* 用户区初始化  **********/
	DWT_Init();    //调试计数器
	
  Offset_Current_Start();  //校准作用,电流传感器的理论偏移值为1.65V
	
	//SEGGER_RTT_Port_Init();
	//JS_RTT_PLUS_WaitForConfig();
	
    SoftI2C_RegisterAndInit(1000000,SystemCoreClock);
    bsp_as5600Init();

//	//校准传感器与电机零点偏移角度
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
		HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);
		HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
		HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
		HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
		HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);

		TIM1->CCR1 = PWM_HalfPerMax * 13/10;  //  A  U   // d轴强拖，形成SVPWM模型中的基础矢量1，即对应转子零度位置
		TIM1->CCR2 = PWM_HalfPerMax;  //  B  V
		TIM1->CCR3 = PWM_HalfPerMax;  //  C  W
		HAL_Delay(200);     
		float angle_zero_offset;
		bsp_as5600GetAngle(&angle_zero_offset);
		Encoder_AS5600.angle_zero_offset = - angle_zero_offset;

    TIM1->CCR1 = PWM_HalfPerMax;  //  A  U   // 松开电机
		TIM1->CCR2 = PWM_HalfPerMax;  //  B  V
		TIM1->CCR3 = PWM_HalfPerMax;  //  C  W

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
	uint32_t previousMillis = 0;
  const uint32_t interval = 10; // 单位 ms 间隔
	
			 
  while (1)
  {
			 // 获取当前的系统时间
			 uint32_t currentMillis = HAL_GetTick();

		 // 检查是否已达到间隔时间
		 if (currentMillis - previousMillis >= interval) {
				previousMillis = currentMillis;  // 更新上次执行的时间
			 
			 
        // 调用要定时执行的函数
			 ADCSampPare.RP_speed_Voltage = ADC_ConvertedValue[0];
			 knob_control( );   		//检测电位器

//			 __disable_irq();
//				start = DWT->CYCCNT; // 记录起始时间
//       
//			  bsp_as5600GetAngle(&Encoder_AS5600.angle);
////			  //bsp_as5600_DMAGetAngle(&Encoder_AS5600.angle);
//			 Encoder_AS5600.angle = fmodf(Encoder_AS5600.angle + Encoder_AS5600.angle_zero_offset, Two_PI);  // fmodf 保留符号，所以我们要修正负值
//				if (Encoder_AS5600.angle < 0.0f) Encoder_AS5600.angle += Two_PI;
//				Encoder_AS5600.eleangle	= fmodf(Encoder_AS5600.angle * Pole, Two_PI);
//				end = DWT->CYCCNT;   // 记录结束时间
//				
//				cycles  = end - start; // 计算运行的 CPU 周期数
//				// 处理溢出情况（如果测量过程中发生了溢出）
//				if (end < start) {
//						cycles = (0x100000000ULL - start) + end;  // 处理溢出
//				} else {
//					cycles = end - start;
//				}
//				time_us  = (float)cycles / (SystemCoreClock / 1e6); // 转换为微秒
//				__enable_irq();
				
			
			 //disable_erq();   float FOC_time_us = time_us; void (* FOC_func_ptr)(ADC_HandleTypeDef *hadc) = func_ptr;enable_erq();
//			  myprintf("Function execution time: %.3f us\nFunction %p is in %s memory\n",\
//				FOC_time_us,FOC_func_ptr,((uint32_t)func_ptr >= SRAM_BASE) ? "RAM" : "Flash"); 
			 
			  //HAL_IWDG_Refresh(&hiwdg);  // 喂狗，重装载计数器
			// UploadData();	

		 }
		
  }
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM4)       //判断是否为定时器4产生的中断
    {
		  //T_s_overflag = 1 ;
    }
}

void Param_init(void)
{ 
	  current_q_pid_Init(&current_q_pid, 0.0018f, 3.2f, Udc);
	  current_d_pid_Init(&current_d_pid, 0.0018f, 3.2f, Udc);
		speed_pid_init(&speed_pid, 0.0000075f, 0.017f, 2.0f, Hall_Three.Speed_Calculate_Times);
	
		//ThreeHallPara_init();   // 三霍尔角度传感器的参数初始化
	  bsp_as5600Init();
		SEGGER_RTT_TimeStamp_reset(); 
}

void Param_deinit(void)
{

	memset(&TestPare, 0, sizeof(TestPare));
	memset(&TaskTimePare, 0, sizeof(TaskTimePare));
	memset(&logicContr, 0, sizeof(logicContr));
	memset(&AngleSin_Cos, 0, sizeof(AngleSin_Cos));
	memset(&Hall_Three, 0, sizeof(Hall_Three));
	//memset(&ADCSampPare, 0, sizeof(ADCSampPare));
	
	memset(&foc_dq_v, 0, sizeof(foc_dq_v));
	memset(&foc_dq_i, 0, sizeof(foc_dq_i));
	memset(&current_q_pid, 0, sizeof(current_q_pid));
	memset(&current_d_pid, 0, sizeof(current_d_pid));
	memset(&speed_pid, 0, sizeof(speed_pid));
}


/**********************  DWT  ************************/
void DWT_Init(void) {
    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; // 使能 DWT
    DWT->CYCCNT = 0; // 计数器清零
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; // 使能 CYCCNT 计数器
}


/**********************  ADC  ************************/
ADCSamp      ADCSampPare = ADCSamp_DEFAULTS;
uint16_t  ADC_ConvertedValue[2]={0};
uint32_t adc1_in0, adc1_in1, adc2_in2, adc1_in8, adc1_in3;
//校准作用,电流传感器的理论偏移值为1.65V
//三电阻采样时可以用软件构建动态的偏移电压的ADC值，三相电流的ADC值相加是3倍的偏移电压ADC值的数学关系
void Offset_Current_Start(void)
{
	HAL_TIM_PWM_Stop( &htim1, TIM_CHANNEL_1);
	HAL_TIM_PWM_Stop( &htim1, TIM_CHANNEL_2);
	HAL_TIM_PWM_Stop( &htim1, TIM_CHANNEL_3);
	HAL_TIMEx_PWMN_Stop( &htim1, TIM_CHANNEL_1);
	HAL_TIMEx_PWMN_Stop( &htim1, TIM_CHANNEL_2);
	HAL_TIMEx_PWMN_Stop( &htim1, TIM_CHANNEL_3);
	
	MotorState.run_state = RUNSTATE_CALIBRATING;
	HAL_TIM_PWM_Start( &htim1, TIM_CHANNEL_4);
	HAL_ADCEx_InjectedStart_IT(&hadc1);  //开启FOC运行
	HAL_ADCEx_InjectedStart(&hadc2);
	
	HAL_Delay(200);
}

void Offset_CurrentReading(void)
{
	static uint16_t i;  
  i++;
	
	/* ADC Channel used for current reading are read  in order to get zero currents ADC values*/
	ADCSampPare.OffsetBUS_Curr +=  ADC1->JDR2;
	ADCSampPare.OffsetPhaseV_Curr +=  ADC1->JDR1;;
	ADCSampPare.OffsetPhaseU_Curr += ADC2->JDR1;		 
  
	if(i >= 1000)   
  {
		MotorState.run_state = RUNSTATE_STOPPED;
		i = 0;
		
		ADCSampPare.OffsetBUS_Curr = ADCSampPare.OffsetBUS_Curr / 1000;  
		ADCSampPare.OffsetPhaseV_Curr= ADCSampPare.OffsetPhaseV_Curr / 1000; 
		ADCSampPare.OffsetPhaseU_Curr=ADCSampPare.OffsetPhaseU_Curr / 1000;
		
		HAL_TIM_PWM_Stop( &htim1, TIM_CHANNEL_4);
		HAL_ADCEx_InjectedStop_IT(&hadc1);  //停止FOC运行
		HAL_ADCEx_InjectedStop(&hadc2);	
  }
}


void ADC_Sample(void)
{
	adc1_in1 = HAL_ADCEx_InjectedGetValue( &hadc1, ADC_INJECTED_RANK_1);
	adc1_in0 = HAL_ADCEx_InjectedGetValue( &hadc1, ADC_INJECTED_RANK_2);
	adc2_in2  = HAL_ADCEx_InjectedGetValue( &hadc2, ADC_INJECTED_RANK_1);
	
	/*Uoffset - IV-V = 0.2IV-I   采样电阻为0.1欧姆，放大电路放大倍数2倍 偏移电压减去测量的电压等于0.2倍该路电流,FOC 中通常定义流入电机绕组的电流为正（符合右手定则）
	  低端采样时，相电流流经采样电阻进入PGND（下桥臂导通）时，该电流方向通常是“流出电机”，在 FOC 定义中应为负值,
	 因此这里在计算后的电流前加一个负号  
	
	  如果流入电机的相电流定义为正，则 Iq > 0 时，电机通常逆时针（CCW）旋转。
    Iq < 0 时，电机通常顺时针（CW）旋转。
	*/
	//这里将电流作为Q6.10格式，电流理论值为(adcx_inx-ADCSampPare.Offsetxx)*3.3/4095*5 ≈ (adcx_inx-ADCSampPare.Offsetxx)/2^8
	//我们只需要将电流值赋为(adcx_inx-ADCSampPare.Offsetxx)再左移2位即可转换为Q6.10格式定点数，+-32A符合要求
  ADCSampPare.BUS_Curr  =     - (ADCSampPare.OffsetBUS_Curr - adc1_in0)<<2; //(adc1_in0-ADCSampPare.OffsetBUS_Curr)<<2;	     // 
 	ADCSampPare.PhaseV_Curr  =  - (ADCSampPare.OffsetPhaseV_Curr - adc1_in1)<<2;	  //(adc1_in1-ADCSampPare.OffsetPhaseV_Curr)<<2;	   // 
  ADCSampPare.PhaseU_Curr  =  - (ADCSampPare.OffsetPhaseU_Curr - adc2_in2)<<2;	//(adc2_in2-ADCSampPare.OffsetPhaseU_Curr)<<2;	   //
  //ADCSampPare.RP_speed_Voltage = adc1_in3;	       // 实际值 = ADCSampPare.RP_speed_Voltage/4096*3.3
  //ADCSampPare.BUS_Voltage   = adc1_in8;	         // 实际值 = ADCSampPare.BUS_Voltage/4096*21*3.3
 
	//下式右边的ADCSampPare.BUS_CurrFitter为c(t-1)，左边的ADCSampPare.BUS_CurrFitter为c(t)，Speed_count为r(t)，T为采样时间0.1ms，RC根据截止频率人为设定 */
	ADCSampPare.BUS_CurrFitter =  _IQ10mpy(ADCSampPare.Coeff_filterK1, ADCSampPare.BUS_CurrFitter)+_IQ10mpy(ADCSampPare.Coeff_filterK2, ADCSampPare.BUS_Curr);    
	ADCSampPare.PhaseU_CurrFitter =  _IQ10mpy(ADCSampPare.Coeff_filterK1, ADCSampPare.PhaseU_CurrFitter)+_IQ10mpy(ADCSampPare.Coeff_filterK2, ADCSampPare.PhaseU_Curr);   
	ADCSampPare.PhaseV_CurrFitter =  _IQ10mpy(ADCSampPare.Coeff_filterK1, ADCSampPare.PhaseV_CurrFitter)+_IQ10mpy(ADCSampPare.Coeff_filterK2, ADCSampPare.PhaseV_Curr);   
}


/**********************  UART  ************************/
uint8_t aRxBuffer[100];

void Uart_485_Init(void){
   HAL_GPIO_WritePin(REN485_LED_GPIO_Port, REN485_LED_Pin, GPIO_PIN_RESET);
	 HAL_UARTEx_ReceiveToIdle_DMA(&huart3, aRxBuffer, sizeof(aRxBuffer));  //��������3��DMA�����ж�
	 __HAL_DMA_DISABLE_IT(&hdma_usart3_rx, DMA_IT_HT);  //ȡ���������ݹ����ж�
}
//===========================================================================
// No more.
//===========================================================================
